#ifndef _GRID_MAP_H
#define _GRID_MAP_H

#include <Eigen/Eigen>
#include <cv_bridge/cv_bridge.h>
// #include <opencv2/core.hpp> # TODO
#include <geometry_msgs/PoseStamped.h>
#include <iostream>
#include <nav_msgs/Odometry.h>
#include <queue>
#include <random>
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>

#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/time_synchronizer.h>

#include <plan_env/raycast.h>
#include <plan_env/colors.h>

#define logit(x) (log((x) / (1 - (x))))

using namespace std;

// voxel hashing
template <typename T>
struct matrix_hash : std::unary_function<T, size_t>
{
  std::size_t operator()(T const &matrix) const
  {
    size_t seed = 0;
    for (size_t i = 0; i < matrix.size(); ++i)
    {
      auto elem = *(matrix.data() + i);
      seed ^= std::hash<typename T::Scalar>()(elem) +
              0x9e3779b9 + (seed << 6) + (seed >> 2);
    }
    return seed;
  }
};

/* constant parameters */
struct GridMapParameters
{
  /* map properties */
  Eigen::Vector3d map_origin_, map_size_;
  Eigen::Vector3d map_min_boundary_, map_max_boundary_; // map range in pos
  Eigen::Vector3i map_voxel_num_;                       // map range in index
  Eigen::Vector3d local_update_range_;
  double resolution_, resolution_inv_;
  double obstacles_inflation_; // size of UAV
  string frame_id_;
  int pose_type_;

  /* esdf map bound (pixel)*/
  int esdf_x_bound_, esdf_y_bound_, esdf_z_bound_;

  /* camera parameters */
  double cx_, cy_, fx_, fy_;

  /* time out */
  double odom_depth_timeout_;

  /* depth image projection filtering */
  double depth_filter_maxdist_, depth_filter_mindist_, depth_filter_tolerance_;
  int depth_filter_margin_;
  bool use_depth_filter_;
  double k_depth_scaling_factor_;
  int skip_pixel_;

  /* raycasting */
  double p_hit_, p_miss_, p_min_, p_max_, p_occ_; // occupancy probability
  double prob_hit_log_, prob_miss_log_, clamp_min_log_, clamp_max_log_,
      min_occupancy_log_;                  // logit of occupancy probability
  double min_ray_length_, max_ray_length_; // range of doing raycasting

  /* active mapping */
  double unknown_flag_;

  /* local map update and clear */
  int local_map_margin_;

  /* visualization display */
  double visual_truncate_height_;
  double virtual_ceil_height_, ground_height_;
};

/* intermediate mapping data for fusion */
struct GridMapData
{
  /* main map data, occupancy of each voxel and Euclidean distance */
  std::vector<double> occupancy_buffer_;
  std::vector<int8_t> occupancy_buffer_inflate_;
  std::vector<int8_t> occupancy_buffer_test_; // TODO TEST
  std::vector<int8_t> occupancy_buffer_neg;

  std::vector<double> tmp_buffer1_;
  std::vector<double> tmp_buffer2_;
  std::vector<double> distance_buffer_;
  std::vector<double> distance_buffer_all_;
  std::vector<double> distance_buffer_neg_;

  Eigen::Vector3i min_esdf_{0, 0, 0};
  Eigen::Vector3i max_esdf_{0, 0, 0};

  int buffer_size_;

  /* local updating bound in grid */
  Eigen::Vector3i local_bound_min_, local_bound_max_;

  /* camera position and pose data */
  Eigen::Vector3d camera_pos_, last_camera_pos_;
  Eigen::Matrix3d camera_r_m_, last_camera_r_m_;
  /* depth image data */
  cv::Mat depth_image_;
  /* points in world frame projected from   depth image*/
  std::vector<Eigen::Vector3d> proj_pts_;
  int proj_pts_cnt_;

  /* flags of map state */
  bool occ_need_update_, local_updated_, esdf_need_update_;
  bool has_first_depth_;
  bool has_odom_, has_cloud_;

  /* odom_depth_timeout_ */
  ros::Time last_occ_update_time_;
  bool flag_depth_odom_timeout_;
  bool flag_use_depth_fusion;

  /* flag buffers for speeding up raycasting */
  std::queue<Eigen::Vector3i> cache_voxel_;
  std::vector<short> count_hit_, count_hit_and_miss_;
  std::vector<short> flag_traverse_, flag_rayend_;
  short raycast_num_;

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

class GridMap
{
public:
  GridMap() {}
  virtual ~GridMap() {}

  enum {
    POSE_STAMPED = 1,
    ODOMETRY = 2,
    INVALID_IDX = -10000
  };

  virtual void initMap(ros::NodeHandle &nh);

  /* Tool func, overloading*/
  int getOccupancy(Eigen::Vector3d pos);
  int getOccupancy_idx(Eigen::Vector3i idx);
  int getGridState(Eigen::Vector3d pos);
  bool isInESDF(const Eigen::Vector3d &pos);
  bool isInESDF(const Eigen::Vector3i &idx);
  bool roundWithOrigin(Eigen::Vector3d &pos_round);
  void boundIndex(Eigen::Vector3i &id);
  void esdfBoundIndex(Eigen::Vector3i &id);

  double getDistance(const Eigen::Vector3d &pos);
  void getOrigin(Eigen::Vector3d &ori);
  void getRegion(Eigen::Vector3d &ori, Eigen::Vector3d &size);
  double getResolution() { return gmp_.resolution_; };
  double getResolution_inv() { return gmp_.resolution_inv_; };
  bool getOdomDepthTimeout() { return gmd_.flag_depth_odom_timeout_; }

  bool evaluateESDF(const Eigen::Vector3d &pos, double &dist);
  bool evaluateESDFWithGrad(const Eigen::Vector3d &pos,
                            double &dist, Eigen::Vector3d &grad);
  void getSurroundPts(const Eigen::Vector3d &pos,
                      Eigen::Vector3d pts[2][2][2], Eigen::Vector3d &diff);
  void getSurroundDistance(Eigen::Vector3d pts[2][2][2], double dists[2][2][2]);
  void interpolateTrilinear(double values[2][2][2], const Eigen::Vector3d &diff,
                            double &value);
  void interpolateTrilinear(double values[2][2][2], const Eigen::Vector3d &diff,
                            double &value, Eigen::Vector3d &grad);

  void publishMap();
  void publishMapInflate(bool all_info = false);
  void publishESDF();

  typedef std::shared_ptr<GridMap> Ptr;

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

protected:
  GridMapParameters gmp_;
  GridMapData gmd_;

  /* Tool func, overloading*/
  // @warning   confirm the pos is in map before use the func
  void posToIndex(const Eigen::Vector3d &pos, Eigen::Vector3i &id);
  void posToIndex(const Eigen::Vector2d &pos, Eigen::Vector2i &id);
  void indexToPos(const Eigen::Vector3i &id, Eigen::Vector3d &pos);
  void indexToPos(const Eigen::Vector2i &id, Eigen::Vector2d &pos);
  int toAddress(const Eigen::Vector3i &id);
  int toAddress(const Eigen::Vector2i &id);
  int toAddress(int x, int y, int z);
  bool isInMap(const Eigen::Vector3d &pos);
  bool isInMap(const Eigen::Vector2d &pos);
  bool isInMap(const Eigen::Vector3i &idx);
  bool isInMap(const Eigen::Vector2i &idx);
  bool isInMap(int idx);

  /* get depth image and camera pose */
  // void cloudCallback(const sensor_msgs::PointCloud2ConstPtr &img);
  void depthPoseCallback(const sensor_msgs::ImageConstPtr &img,
                         const geometry_msgs::PoseStampedConstPtr &pose);
  void depthOdomCallback(const sensor_msgs::ImageConstPtr &img,
                         const nav_msgs::OdometryConstPtr &odom);

  /* update occupancy by raycasting */
  void updateOccupancyCallback(const ros::TimerEvent & /*event*/);
  void updateESDFCallback(const ros::TimerEvent & /*event*/);
  void updateESDF3d();
  virtual void visCallback(const ros::TimerEvent & /*event*/);

  template <typename F_get_val, typename F_set_val>
  void fillESDF(F_get_val f_get_val, F_set_val f_set_val, int start, int end, int dim);

  // main update process
  virtual void projectDepthImage();
  void raycastProcess();
  void clearAndInflateLocalMap();

  inline void inflatePoint(const Eigen::Vector3i &pt, int step,
                           vector<Eigen::Vector3i> &pts);
  int setCacheOccupancy(Eigen::Vector3d pos, bool isOcc);
  Eigen::Vector3d closetPointInMap(const Eigen::Vector3d &pt,
                                   const Eigen::Vector3d &camera_pt);
  Eigen::Vector2d closetPointInMap(const Eigen::Vector2d &pt,
                                   const Eigen::Vector2d &camera_pt);

  typedef message_filters::sync_policies::ApproximateTime<
      sensor_msgs::Image, nav_msgs::Odometry>
      SyncPolicyImageOdom;
  typedef message_filters::sync_policies::ApproximateTime<
      sensor_msgs::Image, geometry_msgs::PoseStamped>
      SyncPolicyImagePose;
  typedef message_filters::Synchronizer<SyncPolicyImagePose> SynchronizerImagePose;
  typedef message_filters::Synchronizer<SyncPolicyImageOdom> SynchronizerImageOdom;
  typedef message_filters::Subscriber<sensor_msgs::Image> mfSubImg;
  typedef message_filters::Subscriber<geometry_msgs::PoseStamped> mfSubPose;
  typedef message_filters::Subscriber<nav_msgs::Odometry> mfSubOdom;

  ros::NodeHandle node_;
  shared_ptr<mfSubImg> depth_sub_;
  shared_ptr<mfSubPose> pose_sub_;
  shared_ptr<mfSubOdom> odom_sub_;
  shared_ptr<SynchronizerImagePose> sync_image_pose_;
  shared_ptr<SynchronizerImageOdom> sync_image_odom_;

  ros::Publisher map_pub_, map_inf_pub_, map_esdf_pub_;
  ros::Timer occ_timer_, ESDF_timer_, vis_timer_;
};

/* -------------------------------------------------------------------------- */
/*                        definition of inline function                       */
/* -------------------------------------------------------------------------- */

inline int GridMap::toAddress(const Eigen::Vector3i &id)
{
  return id(0) * gmp_.map_voxel_num_(1) * gmp_.map_voxel_num_(2) +
         id(1) * gmp_.map_voxel_num_(2) + id(2);
}

inline int GridMap::toAddress(const Eigen::Vector2i &id)
{
  return id(0) * gmp_.map_voxel_num_(1) + id(1);
}

inline int GridMap::toAddress(int x, int y, int z)
{
  return x * gmp_.map_voxel_num_(1) * gmp_.map_voxel_num_(2) +
         y * gmp_.map_voxel_num_(2) + z;
}

inline void GridMap::boundIndex(Eigen::Vector3i &id)
{
  Eigen::Vector3i id1;
  id1(0) = max(min(id(0), gmp_.map_voxel_num_(0) - 1), 0);
  id1(1) = max(min(id(1), gmp_.map_voxel_num_(1) - 1), 0);
  id1(2) = max(min(id(2), gmp_.map_voxel_num_(2) - 1), 0);
  id = id1;
}

inline bool GridMap::isInESDF(const Eigen::Vector3d &pos)
{
  Eigen::Vector3i id;
  posToIndex(pos, id);

  return isInESDF(id);
}

inline bool GridMap::isInESDF(const Eigen::Vector3i &idx)
{
  if (idx(0) < (gmd_.min_esdf_(0) + 2) ||
      idx(1) < (gmd_.min_esdf_(1) + 2) ||
      idx(2) < (gmd_.min_esdf_(2) + 1))
  {
    return false;
  }
  if (idx(0) > (gmd_.max_esdf_(0) - 2) ||
      idx(1) > (gmd_.max_esdf_(1) - 2) ||
      idx(2) > (gmd_.max_esdf_(2) - 1))
  {
    return false;
  }
  return true;
}

inline void GridMap::esdfBoundIndex(Eigen::Vector3i &id)
{
  Eigen::Vector3i id1;
  id1(0) = max(min(id(0), gmd_.max_esdf_(0) - 1), gmd_.min_esdf_(0));
  id1(1) = max(min(id(1), gmd_.max_esdf_(1) - 1), gmd_.min_esdf_(1));
  id1(2) = max(min(id(2), gmd_.max_esdf_(2) - 1), gmd_.min_esdf_(2));
  id = id1;
}

// TODO MZY 在边界上他可能只是找了一个就近点
/* get distance from esdf */
inline double GridMap::getDistance(const Eigen::Vector3d &pos)
{
  Eigen::Vector3i id;
  posToIndex(pos, id);
  esdfBoundIndex(id);
  return gmd_.distance_buffer_all_[toAddress(id)];
}

/**
 * @return -2: out of map range;
 *         -1: ;
 *          0: free or unknown;
 *          1: occupied
 *
 * TODO 与getGridState合并
 */
inline int GridMap::getOccupancy(Eigen::Vector3d pos)
{
  if (!isInMap(pos))
    return -2;

  Eigen::Vector3i id;
  posToIndex(pos, id);
  if (gmd_.occupancy_buffer_inflate_[toAddress(id)] == 1) {
    return 1;
  }
  return 0;
}

inline int GridMap::getOccupancy_idx(Eigen::Vector3i idx)
{
  if (!isInMap(idx))
    return -2;

  if (gmd_.occupancy_buffer_inflate_[toAddress(idx)] == 1) {
    return 1;
  }
  return 0;
}

/**
 * @return -2: out of map range;
 *         -1: unknown;
 *          0: free;
 *          1: occupied
 */
inline int GridMap::getGridState(Eigen::Vector3d pos)
{
  if (!isInMap(pos))
    return -2;

  Eigen::Vector3i id;
  posToIndex(pos, id);

  return int(gmd_.occupancy_buffer_inflate_[toAddress(id)]);
}

inline bool GridMap::isInMap(const Eigen::Vector3d &pos)
{
  if (pos(0) < gmp_.map_min_boundary_(0) + 1e-4 ||
      pos(1) < gmp_.map_min_boundary_(1) + 1e-4 ||
      pos(2) < gmp_.map_min_boundary_(2) + 1e-4)
  {
    return false;
  }
  if (pos(0) > gmp_.map_max_boundary_(0) - 1e-4 ||
      pos(1) > gmp_.map_max_boundary_(1) - 1e-4 ||
      pos(2) > gmp_.map_max_boundary_(2) - 1e-4)
  {
    return false;
  }
  return true;
}

inline bool GridMap::isInMap(const Eigen::Vector2d &pos)
{
  if (pos(0) < gmp_.map_min_boundary_(0) + 1e-4 ||
      pos(1) < gmp_.map_min_boundary_(1) + 1e-4)
  {
    return false;
  }
  if (pos(0) > gmp_.map_max_boundary_(0) - 1e-4 ||
      pos(1) > gmp_.map_max_boundary_(1) - 1e-4)
  {
    return false;
  }
  return true;
}

inline bool GridMap::isInMap(const Eigen::Vector3i &idx)
{
  if (idx(0) < 0 || idx(1) < 0 || idx(2) < 0)
  {
    return false;
  }
  if (idx(0) > gmp_.map_voxel_num_(0) - 1 ||
      idx(1) > gmp_.map_voxel_num_(1) - 1 ||
      idx(2) > gmp_.map_voxel_num_(2) - 1)
  {
    return false;
  }
  return true;
}

inline bool GridMap::isInMap(const Eigen::Vector2i &idx)
{
  if (idx(0) < 0 || idx(1) < 0)
  {
    return false;
  }
  if (idx(0) > gmp_.map_voxel_num_(0) - 1 ||
      idx(1) > gmp_.map_voxel_num_(1) - 1)
  {
    return false;
  }
  return true;
}

inline bool GridMap::isInMap(int idx)
{
  if (idx < 0)
  {
    return false;
  }
  if (idx >
      gmp_.map_voxel_num_(0) * gmp_.map_voxel_num_(1) * gmp_.map_voxel_num_(2) - 1)
  {
    return false;
  }
  return true;
}

inline void GridMap::posToIndex(const Eigen::Vector3d &pos, Eigen::Vector3i &id)
{
  for (int i = 0; i < 3; ++i)
    id(i) = floor((pos(i) - gmp_.map_origin_(i)) * gmp_.resolution_inv_);
}

inline void GridMap::posToIndex(const Eigen::Vector2d &pos, Eigen::Vector2i &id)
{
  for (int i = 0; i < 2; ++i)
    id(i) = floor((pos(i) - gmp_.map_origin_(i)) * gmp_.resolution_inv_);
}

inline void GridMap::indexToPos(const Eigen::Vector3i &id, Eigen::Vector3d &pos)
{
  for (int i = 0; i < 3; ++i)
    pos(i) = (id(i) + 0.5) * gmp_.resolution_ + gmp_.map_origin_(i);
}

inline void GridMap::indexToPos(const Eigen::Vector2i &id, Eigen::Vector2d &pos)
{
  for (int i = 0; i < 2; ++i)
    pos(i) = (id(i) + 0.5) * gmp_.resolution_ + gmp_.map_origin_(i);
}

inline void GridMap::inflatePoint(const Eigen::Vector3i &pt, int step,
                                  vector<Eigen::Vector3i> &pts)
{
  int num = 0;
  for (int x = -step; x <= step; ++x)
    for (int y = -step; y <= step; ++y)
      for (int z = -step; z <= step; ++z)
      {
        pts[num++] = Eigen::Vector3i(pt(0) + x, pt(1) + y, pt(2) + z);
      }
}

inline void GridMap::getOrigin(Eigen::Vector3d &ori)
{
  ori = gmp_.map_origin_;
}

inline void GridMap::getRegion(Eigen::Vector3d &ori, Eigen::Vector3d &size)
{
  ori = gmp_.map_origin_, size = gmp_.map_size_;
}

/**
 * Ensure that the distance between the point and the origin
 *  is an integer multiple of the grid spacing
 */
inline bool GridMap::roundWithOrigin(Eigen::Vector3d &pos)
{
  Eigen::Vector3i id;
  posToIndex(pos, id);
  if (!isInMap(id)) {
    cout << "pos" << pos.transpose() << endl;
    cout << "id" << id.transpose() << endl;
    cerr << "\033[1;31m"
         << "[GridMap]: roundWithOrigin: pos is not in map!"
         << "\033[0m" << endl;
    return false;
  }

  pos = gmp_.map_origin_ + id.cast<double>() * gmp_.resolution_;

  return true;
}

#endif